
from dis import dis
from platform import node
from re import U
from cv2 import exp
from keyring import set_keyring
import rclpy
from rclpy.node import Node
#msg
from carsource_interfaces.msg import GlobalPathPlanning
from carsource_interfaces.msg import Goal
from geometry_msgs.msg import Pose
import yaml

from transforms3d.euler import quat2euler
from transforms3d.euler import euler2quat

from PIL import Image
import cv2
import numpy as np

import math
import matplotlib.pyplot as plt

import time
class Config(object):
    """
    用来仿真的参数，
    """

    def __init__(self):
        # robot parameter
        self.max_speed = 0.2  # [m/s]  # 最大速度
        # self.min_speed = -0.5  # [m/s]  # 最小速度，设置为可以倒车
        self.min_speed = 0  # [m/s]  # 最小速度，设置为不倒车
        self.max_yawrate = math.radians(1)  # [rad/s]  # 最大角速度
        self.max_accel = 0.2  # [m/ss]  # 最大加速度
        self.max_dyawrate = math.radians(1) # [rad/ss]  # 最大角加速度
        self.v_reso = 0.05  # [m/s]，速度分辨率
        self.yawrate_reso = math.radians(0.1)  # [rad/s]，角速度分辨率
        self.dt = 0.1  # [s]  # 采样周期
        self.predict_time = 3.0  # [s]  # 向前预估三秒
        self.to_goal_cost_gain = 1.0  # 目标代价增益
        self.speed_cost_gain = 1.0  # 速度代价增益
        self.robot_radius = 0.05  # [m]  # 机器人半径

class LocalPathNode(Node):

    def __init__(self, name, map_path):
        super().__init__(name)

        self.resolution = 0 # 地图 分辨率

        self.obstacle_map = None #膨胀地图
        self.ox = [] #全局地图的障碍物x轴坐标
        self.oy = [] #全局地图的障碍物y轴坐标
        self.origin = []

        self.velocity_base = 0.2
        self.rad_velocity_base = 0
        self.config = Config()

        self.min_x = 0
        self.min_y = 0
        self.max_x = 0
        self.max_y = 0
        self.x_width = 0
        self.y_width = 0

        self.global_path_x = [] #全局路径的x轴数组
        self.global_path_y = [] #全局路径的y轴数组

        self.expansion_meter = 0.8  # 1m

        self.cur_pose = [238,242] #机器人当前位置 x y
       
        self.cur_orientation = [1,0,0,0] #机器人当前位置 y

        yaml_path = map_path + "map.yaml"
        pgm_path = map_path + "map.pgm"
        

        self.resolution, self.origin = self.yaml_read(yaml_path)
        self.img_ = self.read_img(pgm_path)
             
        self.get_obstacle()


        plt.plot(self.ox, self.oy, ".k")
        plt.plot(238, 242, "og")
        
        plt.grid(True)
        plt.axis("equal")


        ax = plt.gca()                                 #获取到当前坐标轴信息
        ax.xaxis.set_ticks_position('top')   #将X坐标轴移到上面
        ax.invert_yaxis()  

        #NOTE 调试 后续可以去掉
        self.calc_obstacle_map() # 
        
        # print("what the fuck")
        print("障碍物加载成功")  
  
        #subscriber
        self.sub_global_path = self.create_subscription(GlobalPathPlanning,"global_path_planning",self.recv_global_path,1)
        self.current_pos_sub = self.create_subscription(Pose,"current_pose", self.recv_curr_pose,1)
        #publish
        self.current_pos_pub = self.create_publisher(Pose,"current_pose", 1) 
        #time callback
        self.timer = self.create_timer(0.25, self.local_path_planning)

    def yaml_read(self, yaml_path):
        f = open(yaml_path,'r',encoding='utf-8')
        cont = f.read()
        f_info = yaml.load(cont)
        
        resolution = f_info['resolution']
        
        origin = f_info['origin']

        return resolution,origin

    def read_img(self,pgm_name):
        img_ = cv2.imread(pgm_name,0)
        # cv2.namedWindow('img_', cv2.WINDOW_FREERATIO)   # 窗口大小自适应比例
        # cv2.imshow("img_", img_)   
        return img_

    def get_obstacle(self):
        height, width= self.img_.shape

        for i in range(height):
            for j in range(0,width):
                if(self.img_[i,j] == 0):
                    
                    self.ox.append(j)
                    self.oy.append(i)

    # 加载动态窗口
    def calc_obstacle_map(self):

        height, width= self.img_.shape
        # 只让 DWA 关注当前一定范围的膨胀地图系数 0.8/0.05 - 16格 16x16次的循环
        grid_num = self.expansion_meter / self.resolution

        self.min_x = round(max(0,self.cur_pose[0] - grid_num))
        self.min_y = round(max(0,self.cur_pose[1] - grid_num))
        self.max_x = round(min(height,self.cur_pose[0] + grid_num))
        self.max_y = round(min(width,self.cur_pose[1] + grid_num))
        
 
        self.x_width = round((self.max_x - self.min_x))
        self.y_width = round((self.max_y - self.min_y))

        # print("ob_stacle_x_width:", self.x_width)
        # print("ob_stacle_y_width:", self.y_width)
        # print("----------------------------------------------")
        # obstacle map generation
        # 生成膨胀地图 

        self.obstacle_map = [[False for _ in range(self.y_width)]
                            for _ in range(self.x_width)]
        for ix in range(self.x_width):
            for iy in range(self.y_width):
                for iox, ioy in zip(self.ox, self.oy):
                    d = math.hypot((iox - self.min_x - ix) * self.resolution, (ioy - self.min_y - iy)* self.resolution)
                    if d <= self.config.robot_radius:
                        plt.plot(ix,iy,'-r')
                        self.obstacle_map[ix][iy] = True
                        break
        # plt.show()
    def recv_global_path(self,msg:GlobalPathPlanning):
        
        self.global_path_x = msg.x_pos
        self.global_path_y = msg.y_pos

    def recv_curr_pose(self, msg:Pose):
        
        

        self.cur_pose[0] = msg.position.x
        self.cur_pose[1] = msg.position.y
        

        self.cur_orientation[0] = msg.orientation.w
        self.cur_orientation[1] = msg.orientation.x
        self.cur_orientation[2] = msg.orientation.y
        self.cur_orientation[3] = msg.orientation.z 

        self.calc_obstacle_map()

    def local_path_planning(self):
        # print("dwa sim begin")
        if(len(self.global_path_x) == 0):
            return

        print("dwa sim begin")

        goal_index = [0,0]
        start_index = 0
       
        dist = 1000
        for i in range(len(self.global_path_x)):
            d = math.hypot(self.global_path_x[i] - self.cur_pose[0],self.global_path_y[i] - self.cur_pose[1]) * self.resolution
            # print(dist)
            # 搜索全局路径和当前坐标点最近的点
            if(d <= dist): 
                dist = d
                start_index = i

        goal_index[0] = self.global_path_x[start_index]
        goal_index[1] = self.global_path_y[start_index]

        cur_goal_dis = math.hypot(goal_index[0] - self.cur_pose[0],goal_index[1] - self.cur_pose[1]) * self.resolution

        
        if(cur_goal_dis < 0.08):
                
            self.global_path_x = self.global_path_x[start_index + 1:]
            self.global_path_y = self.global_path_y[start_index + 1:]
            if(len(self.global_path_x) > 0):

                print("reach sub goal continue navigation")


        print(len(self.global_path_x))
        start_index = start_index + 1

        self.global_path_x = self.global_path_x[start_index :]
        self.global_path_y = self.global_path_y[start_index :]
        

        plt.plot(goal_index[0],goal_index[1], "xb")
        # print(goal_index)       
               
        vr = self.calc_dynamic_window() #前进速度与角速度
        
        # print(vr)
        # 模拟轨迹
        u, trajectory = self.calc_final_input(vr, goal_index)

       
        self.pub_pos(trajectory)

   
        

        # self.global_path_x.pop(0)
        # self.global_path_y.pop(0)

    def pub_pos(self,trajectory_):
         # print(trajectory)
        pose_msg = Pose()

        q = euler2quat(0,0,trajectory_[5][2], 'rxyz')

        pose_msg.position.x = trajectory_[5][0]
        pose_msg.position.y = trajectory_[5][1]
        pose_msg.position.z = 0.0

        pose_msg.orientation.w = q[0]
        pose_msg.orientation.x = q[1]
        pose_msg.orientation.y = q[2]
        pose_msg.orientation.z = q[3]

        plt.ion()
        plt.plot(trajectory_[10][0], trajectory_[10][1], ".y")
        plt.pause(0.01)
        plt.show()

        self.current_pos_pub.publish(pose_msg)


    def calc_dynamic_window(self):
        """
        位置空间集合
        :param x:当前位置空间,符号参考硕士论文
        :param config:
        :return:目前是两个速度的交集，还差一个
        """

        # 车辆能够达到的最大最小速度
    
        vs = [self.config.min_speed, self.config.max_speed,
            -self.config.max_yawrate, self.config.max_yawrate]

        print("vs:",vs)
        # 一个采样周期能够变化的最大最小速度

        vd = [self.velocity_base - self.config.max_accel * self.config.predict_time,
              self.velocity_base + self.config.max_accel * self.config.predict_time,
              self.rad_velocity_base  - self.config.max_dyawrate * self.config.predict_time,
              self.rad_velocity_base  + self.config.max_dyawrate * self.config.predict_time]
        
        print("vd:",vd)
        #  print(Vs, Vd)

        # 求出两个速度集合的交集
        vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
            max(vs[2], vd[2]), min(vs[3], vd[3])]

        print("vr:",vr)
        return vr

    def calc_final_input(self, vr, goal):
        """
        计算采样空间的评价函数，选择最合适的那一个作为最终输入
        :param x:位置空间
        :param u:速度空间
        :param vr:速度空间交集
        :param config:
        :param goal:目标位置
        :param ob:障碍物
        :return:
        """
        print("self.cur_pose :", self.cur_pose)
        pose_init = [[],[],[]]
        pose_init[0] = self.cur_pose[0]
        pose_init[1] = self.cur_pose[1]
        yaw = self.quart_to_yaw(self.cur_orientation) 
        pose_init[2] = yaw

  

        print("pose_init:",pose_init)

        v_w = [0.2,0] # 之后路径对应的速度和角速度 
        
        
        position = pose_init  + v_w

        # print(position)
        min_cost = 10000.0

        best_trajectory = np.array([position])

        # evaluate all trajectory with sampled input in dynamic window
        # v,生成一系列速度，w，生成一系列角速度
        start = time.time()
        print("v_min, v_max",vr[0], vr[1])

        print("yaw_min, yaw_max",vr[2], vr[3])

        for v in np.arange(vr[0], vr[1], self.config.v_reso):
            for w in np.arange(vr[2], vr[3], self.config.yawrate_reso):
                # cound_traj = cound_traj + 1
                
                trajectory = self.calc_trajectory(position, v, w, self.config)

                # plt.plot(trajectory[:, 0], trajectory[:, 1], "-g")
                # print(trajectory)
                # calc cost
                to_goal_cost = self.calc_to_goal_cost(trajectory, goal)
                speed_cost = self.config.speed_cost_gain * (self.config.max_speed - trajectory[-1, 3])
                ob_cost = self.calc_obstacle_cost(trajectory)
                #  print(ob_cost)

                # 评价函数多种多样，看自己选择
                # 本文构造的是越小越好
                final_cost = to_goal_cost + speed_cost + ob_cost

                # search minimum trajectory
                if min_cost >= final_cost:
                    min_cost = final_cost
                    v_w[0] = v
                    v_w[1] = w
                    best_trajectory = trajectory
        
        plt.plot(best_trajectory[:,0], best_trajectory[:,1], "-r")
        end = time.time()
        # print(best_trajectory)
        # print(cound_traj)
        # print(min_u)
        #  input()

        return v_w,best_trajectory

    def calc_to_goal_cost(self,trajectory, goal):
        """
        计算轨迹到目标点的代价
        :param trajectory:轨迹搜索空间
        :param goal:
        :param config:
        :return: 轨迹到目标点欧式距离
        """
        # calc to goal cost. It is 2D norm.
        
        dx = goal[0] - trajectory[-1][0]
        dy = goal[1] - trajectory[-1][1]
        goal_dis = math.sqrt(dx ** 2 + dy ** 2)
        cost = self.config.to_goal_cost_gain * goal_dis

        return cost
    

    def calc_obstacle_cost(self, traj):
        """
        计算预测轨迹和障碍物的最小距离，dist(v,w)
        :param traj:
        :param ob:
        :param config:
        :return:
        """
        # calc obstacle cost inf: collision, 0:free

        min_r = float("inf")  # 距离初始化为无穷大
        # start = time.time()
        for ii in range(0, len(traj[:, 1])):
            for i in range(self.x_width):
                for j in range(self.y_width):
                    if(self.obstacle_map[i][j] == True):
                        
                        ox_ = i + self.min_x
                        oy_ = j + self.min_y
                        dx_ = traj[ii, 0] - ox_
                        dy_ = traj[ii, 1] - oy_

                        r = math.sqrt(dx_ ** 2 + dy_ ** 2) * self.resolution
                        if r <= self.config.robot_radius:
                            return float("Inf")  # collision

                        if min_r >= r:
                            min_r = r

        # print("当前最近的障碍物距离",min_r) 
        # print("计算时间:%.5f秒"%(end-start))
        return 1.0 / min_r  # 越小越好

    def calc_trajectory(self, position, v, w, config):
        """
        预测3秒内的轨迹
        :param x_init:位置空间
        :param v:速度
        :param w:角速度
        :param config:
        :return: 每一次采样更新的轨迹，位置空间垂直堆叠
        """
        x = np.array(position)
        trajectory = np.array(x)
        time = 0
        
        while time <= self.config.predict_time:

            x = self.motion(x, [v, w], self.config.dt)
            trajectory = np.vstack((trajectory, x))  # 垂直堆叠，vertical
            time += self.config.dt

            

        return trajectory

    def motion(self, x, u, dt):
        """
        :param x: 位置参数，在此叫做位置空间
        :param u: 速度和加速度，在此叫做速度空间
        :param dt: 采样时间
        :return:
        """
        # print((u[0] * math.cos(x[2]) * dt)/self.resolution)
        x[0] += (u[0] * math.cos(math.degrees(x[2])) * dt)/self.resolution  # x方向位移
        x[1] += (u[0] * math.sin(math.degrees(x[2])) * dt)/self.resolution  # y
        x[2] += u[1] * dt  # 航向角
        x[3] = u[0]  # 速度v
        x[4] = u[1]  # 角速度w
        # print(x)

        return x        
    def quart_to_yaw(self, orientation):
            # roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
            # pitch = math.asin(2 * (w * y - x * z))
            yaw = math.atan2(2 * (orientation[0] * orientation[3] + orientation[1] * orientation[2]),
                                1 - 2 * (orientation[3] * orientation[3] + orientation[2] * orientation[2]))
            return yaw


    def DwaSearch(self):

        plt.cla()  # 清除上次绘制图像
        plt.plot(self.ox, self.oy, ".k")
        plt.plot(238, 242, "og")
        
        plt.grid(True)
        plt.axis("equal")
        ax = plt.gca()                                 #获取到当前坐标轴信息
        ax.xaxis.set_ticks_position('top')   #将X坐标轴移到上面
        ax.invert_yaxis()  

    def plot_arrow(self,x, y, yaw, length=0.5, width=0.1):
        """
        arrow函数绘制箭头
        :param x:
        :param y:
        :param yaw:航向角
        :param length:
        :param width:参数值为浮点数，代表箭头尾部的宽度，默认值为0.001
        :return:
        length_includes_head：代表箭头整体长度是否包含箭头头部的长度，默认值为False
        head_width：代表箭头头部的宽度，默认值为3*width，即尾部宽度的3倍
        head_length：代表箭头头部的长度度，默认值为1.5*head_width，即头部宽度的1.5倍
        shape：参数值为'full'、'left'、'right'，表示箭头的形状，默认值为'full'
        overhang：代表箭头头部三角形底边与箭头尾部直接的夹角关系，通过该参数可改变箭头的形状。
        默认值为0，即头部为三角形，当该值小于0时，头部为菱形，当值大于0时，头部为鱼尾状
        """
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                head_length=1.5 * width, head_width=width)
        plt.plot(x, y)

    def rotate(angle,valuex,valuey):
        angle = math.radians(angle)
        rotatex = math.cos(angle)*valuex -math.sin(angle)*valuey
        rotatey = math.cos(angle)*valuey + math.sin(angle)* valuex
        rotatex = rotatex.tolist()
        rotatey = rotatey.tolist()
        xy = rotatex + rotatey
        return xy
def main(args=None):

    rclpy.init(args=args)
    map_path = "/home/maotian/Ros2project/ros2_ws/car_ws/src/car_source/map/"
    node = LocalPathNode("local_path", map_path)

    # while(rclpy.ok()):

        
    #     node.local_path_planning()

    #     rclpy.spin_once(node)
    rclpy.spin(node)    
    # node.destroy_node()
    rclpy.shutdown()

    
if __name__ == '__main__':
    main()
